#include "car.h"
#include "common.h"
#include <iostream>

bool calculateObjectPosition(const Vehicle &v1, const Vehicle &v2,
                             double &obj_x, double &obj_y)
{
    // 计算观测方向总角度
    double dir1 = v1.theta - v1.alpha;
    double dir2 = v2.theta - v2.alpha;
    std::cout << "dir1: " << radiansToDegrees(dir1) << ", dir2: " << radiansToDegrees(dir2) << std::endl;

    // 计算方向余弦/正弦
    double cos1 = cos(dir1);
    double sin1 = sin(dir1);
    double cos2 = cos(dir2);
    double sin2 = sin(dir2);

    double dx = v2.x - v1.x;
    double dy = v2.y - v1.y;

    // 计算行列式
    double D = cos1 * (-sin2) - (-cos2) * sin1;

    // 使用相对误差阈值，避免浮点精度问题
    if (fabs(D) < 1e-6 * (fabs(cos1) + fabs(sin1) + fabs(cos2) + fabs(sin2)))
        return false;

    // 克莱姆法则求解参数
    double t = ((-sin2) * dx - (-cos2) * dy) / D;
    double s = (cos1 * dy - sin1 * dx) / D;

    // 增加射线方向判断：参数t和s必须同时非负
    if (t < -1e-6 || s < -1e-6) {  // 允许微小负值误差
        std::cout << "Invalid t or s value: t=" << t << ", s=" << s << std::endl;
        return false;
    }

    // 计算物体坐标（取车辆1的观测线参数）
    obj_x = v1.x + t * cos1;
    obj_y = v1.y + t * sin1;

    // 新增四舍五入逻辑
    obj_x = std::round(obj_x * 100.0) / 100.0; // 精确到0.01米
    obj_y = std::round(obj_y * 100.0) / 100.0;

    return true;
}
